/*
 * Copyright (c) 2006-2023, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-06-19     RT-Thread    first version
 */

#include <rtthread.h>

#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include "main.h"
#include "dma.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "RC522.h"
#include "stm32f1xx_it.h"

uint8_t readUid[5];
static rt_thread_t tid1 = RT_NULL;
static rt_thread_t tid2 = RT_NULL;
static rt_thread_t tid3 = RT_NULL;
static rt_thread_t tid4 = RT_NULL;
static rt_thread_t tid5 = RT_NULL;
static rt_thread_t tid6 = RT_NULL;
//static rt_thread_t tid7 = RT_NULL;

rt_mailbox_t mymail;
rt_mailbox_t mymai2;
rt_mutex_t mymutex;
rt_mutex_t mymutex1;
struct rt_event event;
uint8_t CardUid[12][4]=
{
    {0xf6,0xe8,0x6b,0xb6},
    {0xfc,0xa2,0xd5,0xcd},
    {0x76,0x63,0x6a,0xb6},
    {0x86,0xbb,0x6d,0xb6},
    {0x5c,0x5e,0xc8,0xcd},
    {0xd6,0x7d,0x78,0xb6},
    {0x5c,0x5e,0xc8,0xcd},
    {0x86,0xbb,0x6d,0xb6},
    {0x76,0x63,0x6a,0xb6},
    {0xfc,0xa2,0xd5,0xcd},
    {0xf6,0xe8,0x6b,0xb6},
    {0x8c,0x42,0xd9,0xcd},
};
/*uint8_t CardUid[12][4]=
{
    {0x0c,0xa9,0xd6,0xcd},
    {0x33,0xc4,0x3c,0x95},
    {0x0c,0xa9,0xd6,0xcd},
    {0x86,0xbb,0x6d,0xb6},
    {0x5c,0x5e,0xc8,0xcd},
    {0xd6,0x7d,0x78,0xb6},
    {0x5c,0x5e,0xc8,0xcd},
    {0x86,0xbb,0x6d,0xb6},
    {0x76,0x63,0x6a,0xb6},
    {0xfc,0xa2,0xd5,0xcd},
    {0xf6,0xe8,0x6b,0xb6},
    {0x8c,0x42,0xd9,0xcd},
};*/
uint8_t CardUid1[12][4]=
{
    {0xf6,0xe8,0x6b,0xb6},
    {0xac,0xac,0x1f,0xe0},
    {0x56,0x70,0x7a,0xb6},
    {0xdc,0x69,0x1e,0xe0},
    {0x4c,0x70,0x0f,0xe0},
    {0x4c,0x75,0x01,0xe1},  //µ½µ×
    {0x4c,0x70,0x0f,0xe0},
    {0xdc,0x69,0x1e,0xe0},
    {0x56,0x70,0x7a,0xb6},
    {0xac,0xac,0x1f,0xe0},
    {0xf6,0xe8,0x6b,0xb6},
    {0x8c,0x42,0xd9,0xcd},
};
uint8_t rfidflag=0,startflag=1;
extern unsigned int panduan;
float kp=5.8,ki=0,kd=3;
int zhongzhi=119,present,timezhi=500; //zhongzhi=119
float error,error_last,ierror,derror;
int16_t pid=0,flag_wait=0;
#define EVENT_FLAG5 (1 << 5)
#define EVENT_FLAG4 (1 << 4)

static void thread_103_entry(void *parameter)
{
    rt_uint32_t e,times=0;
    rt_tick_t tick=0;
    /* 第一次接收事件，事件3或事件5任意一个可以触发线程1，接收完后清除事件标志 */
    while(1)
    {
        if (rt_event_recv(&event, (EVENT_FLAG3 ),
                          RT_EVENT_FLAG_OR |RT_EVENT_FLAG_CLEAR,
                          RT_WAITING_FOREVER, &e) == RT_EOK)
        {

            if(times==0)
            {
                rt_mutex_take(mymutex,RT_WAITING_FOREVER);
                Speed=0;
                Location=2;
            }
            times++;
            rt_kprintf("time:%d",times);
            if(times==1)
            {
                HAL_UART_Transmit(&huart2,(uint8_t *)&("!1#"),3,1000);  //openmv
                rt_thread_mdelay(200);
                HAL_TIM3_CHANNEL_2_DJ(180);   //openmv
                rt_event_send(&event, EVENT_FLAG5);
                rt_thread_mdelay(4000);
                while(panduan!=swj)      //ÉÏÎ»»ú·¢ËÍµÄÖµºÍopenmvµÄ¶þÎ¬ÂëÖµ¶Ô±È
                {
                    rt_kprintf("11111");
                    rt_event_send(&event, EVENT_FLAG5);
                    rt_thread_mdelay(1500);
                    HAL_UART_Transmit(&huart2,(uint8_t *)&("!2#"),3,1000); //×¼±¸pidÖ±×ßºÍopenmvµÄ»ØÕý
                    for(char i=180;i>=90;i--)            //Èç¹û²»ÊÇ¶ÔÓ¦¶þÎ¬Âë,openmv»ºÂý»ØÕý
                    {
                        HAL_TIM3_CHANNEL_2_DJ(i);
                        rt_thread_mdelay(30);
                    }
                    Speed=10;
                    while(timezhi--)zhi1(pid);            //»ØÕý¿ªÊ¼pidÖ±×ßÒ»¶Î
                    timezhi=500;
                    Speed=0;
                    stop();                           //Ö±×ßÍê³ÉÍ£³µ
                    HAL_UART_Transmit(&huart2,(uint8_t *)&("!1#"),3,1000);  //Ö±×ßÍê³Éºóµ½µÚ¶þ¸ö»õÎïÎ»ÖÃ¿ªÊ¼¶þÎ¬ÂëÊ¶±ð
                    HAL_TIM3_CHANNEL_2_DJ(180);   //openmv×ó×ª90¶È
                    rt_event_send(&event, EVENT_FLAG5);
                    rt_thread_mdelay(6000);
                }
                Location=3;
                rt_kprintf("11111");
                HAL_UART_Transmit(&huart2,(uint8_t *)&("!2#"),3,1000);
                rt_thread_mdelay(100);
                rt_event_send(&event, EVENT_FLAG4);
/*                if(swj/1000%10==1)
                {
                    //HAL_UART_Transmit(&huart4,(uint8_t *)&("!+10#"),6,1000);
                }
                else if(swj/1000%10==2)
                {
                   //HAL_UART_Transmit(&huart4,(uint8_t *)&("!+20#"),6,1000);
                }*/
                rt_event_send(&event, EVENT_FLAG5);
                rt_thread_mdelay(100);
                for(char i=180;i>=90;i--)            //ÊÇ¶ÔÓ¦»õÎï,openmv»ØÕý
                {
                    HAL_TIM3_CHANNEL_2_DJ(i);
                    rt_thread_mdelay(30);
                }
            }
            else if(times==2)
            {
                //HAL_UART_Transmit(&huart4,(uint8_t *)&("!+11#"),6,1000);
                //HAL_TIM3_CHANNEL_3_DJ(0);   //¶æ»úÕÅ¿ª
                for(char i=90;i>0;i--)
                {
                    HAL_TIM3_CHANNEL_3_DJ(i);
                    rt_thread_mdelay(30);
                }
                rt_event_send(&event, EVENT_FLAG4);
            }
            else if(times==3)
            {
                //HAL_UART_Transmit(&huart4,(uint8_t *)&("!+11#"),6,1000); //¸æËßc8µÈ´ý¶æ»úÍê³É
                for(char i=0;i<=80;i++)
                {
                    HAL_TIM3_CHANNEL_3_DJ(i);
                    rt_thread_mdelay(30);
                }
                rt_event_send(&event, EVENT_FLAG4);
            }
            else
            {
                Location=4;
                Speed=20;
                times=0;
                rt_event_send(&event, EVENT_FLAG4);
                rt_mutex_release(mymutex);
            }
            //rt_mutex_release(mymutex);
            tick=rt_tick_get();
            rt_kprintf("thread1:%d\n",tick);
        }
        rt_thread_mdelay(100);
    }

}

static void duji_thread_entry(void *parameter)
{
    rt_uint32_t e,flagd=0;
    rt_tick_t tick=0;
    while(1)
    {
        if (rt_event_recv(&event, (EVENT_FLAG5 ),
                          RT_EVENT_FLAG_OR |RT_EVENT_FLAG_CLEAR,
                          RT_WAITING_FOREVER, &e) == RT_EOK)
        {
            flagd++;
            if(flagd==1)
            {
                if(swj/1000%10==1)
                {
                    HAL_TIM3_CHANNEL_1_DJ(130);
                    rt_thread_mdelay(3000);

                }
                else if(swj/1000%10==2)
                {
                    HAL_TIM3_CHANNEL_1_DJ(90); //
                    rt_thread_mdelay(3000);
                }

            }
            else if(flagd==2)
            {
                flagd=0;
                HAL_TIM3_CHANNEL_1_DJ(120); //115
                rt_thread_mdelay(3000);
            }
            tick=rt_tick_get();
            rt_kprintf("%d\n",tick);
        }
        rt_thread_mdelay(1000);
    }

}

static void rc522_thread_entry(void *parameter)
{
    rt_tick_t tick=0;
    uint16_t i;
    while (1)
    {
        if(!readCard(readUid))
        {
            rt_kprintf("id:%x-%x-%x-%x\n",readUid[0],readUid[1],readUid[2],readUid[3]);
            if(swj/10000<4)
            {
                for(i=0;i<4;i++)
                {
                    if(readUid[i]==CardUid[rfidflag][i])
                    {
                        if(i==3)
                        {
                            rfidflag++;
                            if(rfidflag==12)
                            {
                                Speed=0;
                                Location=5;
                            }
                        }
                    }
                    else
                    {
                        break;
                    }
                }
            }
            else if(swj/10000>3)
            {
                for(i=0;i<4;i++)
                {
                    if(readUid[i]==CardUid1[rfidflag][i])
                    {
                        if(i==3)
                        {
                            rfidflag++;
                            if(rfidflag==12)
                            {
                                Speed=0;
                                Location=0;
                            }
                        }
                    }
                    else
                    {
                        break;
                    }
                }
            }
            //rt_event_send(&event, EVENT_FLAG5);
            tick=rt_tick_get();
            rt_kprintf("%d\n",tick);
        }
        rt_thread_mdelay(30);
    }
}

static void bujin_thread_entry(void *parameter)
{
    rt_uint32_t e,flagd=0;
    rt_tick_t tick=0;
    while(1)
    {
        if (rt_event_recv(&event, (EVENT_FLAG4 ),
                          RT_EVENT_FLAG_OR |RT_EVENT_FLAG_CLEAR,
                          RT_WAITING_FOREVER, &e) == RT_EOK)
        {
            flagd++;
            if(flagd==1)
            {
                if(swj/1000%10==1)
                {
                    Motor_BJ_UP(110,0,2); //120

                }
                else if(swj/1000%10==2)
                {
                    Motor_BJ_UP(850,0,3);
                }
            }
            else if(flagd==2)
            {
                Motor_BJ_broadwise(1600,0,0);
            }
            else if(flagd==3)
            {
                Motor_BJ_broadwise(1650,1,0);
            }
            else if(flagd==4)
            {
                flagd=0;
                if(swj/1000%10==1)
                {
                    Motor_BJ_UP(110,1,2);

                }
                else if(swj/1000%10==2)
                {
                    Motor_BJ_UP(850,1,2);
                }
            }
            tick=rt_tick_get();
            rt_kprintf("%d\n",tick);
            rt_thread_mdelay(100);
            if(flagd!=0)
            {
                rt_event_send(&event, EVENT_FLAG3);
            }
        }
        rt_thread_mdelay(100);
    }
}

static void run_thread_entry(void *parameter)
{
    //rt_tick_t tick=0;
    char *str;
    int test;
    while (1)
    {
        //rt_kprintf("12");
        if(flag_wait==1)
        {
            rt_mutex_take(mymutex,RT_WAITING_FOREVER);
            rt_kprintf("");
            if(rt_mb_recv(mymail,(rt_ubase_t *)&str,10) == RT_EOK)
            {
                //tick=rt_tick_get();
                test=(int)str;
                //rt_kprintf("str:%s,test:%d\n",str,test);
                present=test;
                //printf("%d\r\n",Nowdate);
                error_last=error;
                error=(zhongzhi-present);
                ierror+=error;
                if(ierror>30) ierror=30;
                else if(ierror<-30) ierror=-30;
                derror=error-error_last;
                pid=kp*error+ki*ierror+kd*derror;
                if(pid>185)
                {
                  pid=185;
                }
                if(pid<-185)
                {
                  pid=-185;
                }
                rt_kprintf("pid:%d,test:%d\n",pid,test);
                //rt_kprintf("pid:%d,time:%d\n",pid,tick);
                //rt_event_send(&event, EVENT_FLAG3);
            }
            //rt_kprintf("run:%d\n",Date_int);
            if(swj>0)
            {
                zhi(pid);
            }

            if(rfidflag == 1 && startflag == 1)
            {
                startflag++;
                //rt_kprintf("12");
                shache();
                if(swj/10000<=3)
                {
                    rt_kprintf("12           ");
                    left();
                }
                else if(swj/10000>3)
                {
                    rt_kprintf("34             ");
                    right();
                }
                stop();
                rt_thread_mdelay(900);
                while(Date_int>118||Date_int<116)
                {
                    if(Date_int>118)right1();
                    else if(Date_int<116)left1();
                }
            }
            else if(rfidflag == 2 && startflag == 2)
            {
                //startflag++;
                shache();
                if(swj/10000<=3)
                {
                    right();
                }
                else if(swj/10000>3)
                {
                    left();
                }
                while(Date_int>118||Date_int<116)  //120 118
                {
                    if(Date_int>118)right1();
                    else if(Date_int<116)left1();
                }
                switch(swj/10000)
                {
                    case 1: startflag=3;break;
                    case 2: startflag=4;break;
                    case 3: startflag=5;break;
                    case 4: startflag=3;break;
                    case 5: startflag=4;break;
                    case 6: startflag=5;break;
                }
            }
            else if(rfidflag==6&&startflag==6)
            {
                startflag=10;
                shache();
                back(2000); //2300
                stop();
                rt_thread_mdelay(1000);
                while(Date_int>118||Date_int<116)  //120 118
                {
                    if(Date_int>118)right1();
                    else if(Date_int<116)left1();
                }
            }
            else if(rfidflag==10&&startflag==10)
            {
                startflag++;
                shache();
                if(swj/10000<=3)
                {
                    left();
                }
                else if(swj/10000>3)
                {
                    right();
                }
                //left();
                stop();
                rt_thread_mdelay(1000);
                while(Date_int>118||Date_int<116)
                {
                    if(Date_int>118)right1();
                    else if(Date_int<116)left1();
                }
            }
            else if(rfidflag==11&&startflag==11)
            {
                startflag++;
                shache();
                if(swj/10000<=3)
                {
                    right();
                }
                else if(swj/10000>3)
                {
                    left();
                }
                //right();
                stop();
                rt_thread_mdelay(1000);
                while(Date_int>118||Date_int<116)
                {
                    if(Date_int>118)right1();
                    else if(Date_int<116)left1();
                }
            }
            else if(rfidflag==12&&startflag==12)
            {
                startflag=1;
                shache();
                back(2100);
                stop();
                rt_thread_mdelay(1000);
                while(Date_int>118||Date_int<116)
                {
                    if(Date_int>120)right1();
                    else if(Date_int<118)left1();
                }
                stop();
                swj=0;
                panduan=0;
                rfidflag=0;
                flag_wait=0;
                if(flag_time1>=(flag_time))
                {
                    flag_time1=0;
                    //flag_wait=0;
                    flag_time=0;
                }
                else
                {
                    //flag_wait=0;
                    rt_thread_mdelay(10000);
                    if(rt_mb_send(mymai2,(rt_uint32_t)TakeNumbe[flag_time1]) == RT_EOK)
                    {
                        flag_time1++;
                    }
                    else rt_kprintf("send fail\n");
                }
            }
            else if(rfidflag==startflag&&(rfidflag>2&&rfidflag<6))
            {
                startflag=6;
                shache();
                stop();
                rt_event_send(&event, EVENT_FLAG3);
            }
            rt_mutex_release(mymutex);
            rt_thread_mdelay(100);
        }
    }
}

static void wait_thread_entry(void *parameter)
{
    //rt_tick_t tick=0;
    uint16_t a=0;
    char *str;
    while(1)
    {
        rt_mb_recv(mymai2,(rt_ubase_t *)&str,RT_WAITING_FOREVER);
        rt_kprintf("%s,%d,%c\n",str,str,str[0]);
        a=(int)(str[0]-48)*10000+(int)(str[1]-48)*1000+(int)(str[2]-48)*100+(int)(str[3]-48)*10+(int)(str[4]-48)*1;
        //a=(uint16_t)atoi(str);
        rt_kprintf("a=%d\n",a);
        if(a!=swj)
        {
            swj=a;
            Speed=20;
            Location=1;
            flag_wait=1;
            rt_kprintf("%d",swj);
        }
    }

}

/*static void esp_thread_entry(void *parameter)
{
    while(1)
    {
        rt_mutex_take(mymutex1,RT_WAITING_FOREVER);
        rt_kprintf("esp_send");
        ESP_send();
        rt_thread_mdelay(1000);

    }
}*/

int main(void)
{
    rt_err_t result;
    MX_GPIO_Init();
    MX_DMA_Init();
    MX_USART2_UART_Init();
    MX_USART3_UART_Init();
    MX_SPI2_Init();
    MX_TIM2_Init();
    MX_TIM3_Init();

    PcdInit();
    PcdReset();
    ESP_Init();
    rt_thread_mdelay(100);

    mymail = rt_mb_create("myail1", 1, RT_IPC_FLAG_PRIO);
    if(RT_NULL == mymail)
    {
        rt_kprintf("mymail_create fail!\n");
    }
    mymai2 = rt_mb_create("myail2", 8, RT_IPC_FLAG_PRIO);
    if(RT_NULL == mymai2)
    {
        rt_kprintf("mymai2_create fail!\n");
    }
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1); //¶æ»ú
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1); //µç»ú
    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
    HAL_TIM3_CHANNEL_1_DJ(120); //105
    HAL_TIM3_CHANNEL_2_DJ(90);
    HAL_TIM3_CHANNEL_3_DJ(90);
    HAL_UART_Receive_IT(&huart2, (uint8_t *)&Date, 1);
    rt_kprintf("22");
    result = rt_event_init(&event, "event", RT_IPC_FLAG_FIFO);
    if (result != RT_EOK)
    {
        rt_kprintf("init event failed.\n");
        return -1;
    }
    mymutex = rt_mutex_create("my_mutex", RT_IPC_FLAG_FIFO);
    if(RT_NULL == mymutex)
    {
        rt_kprintf("mymutex_create fail!\n");
    }
    mymutex1 = rt_mutex_create("my_mutex1", RT_IPC_FLAG_FIFO);
    if(RT_NULL == mymutex1)
    {
        rt_kprintf("mymutex1_create fail!\n");
    }
    rt_mutex_take(mymutex1,RT_WAITING_FOREVER);
    tid1 = rt_thread_create("thread1",
                            thread_103_entry, RT_NULL,
                            512,
                            25, 5);
    if (tid1 != RT_NULL)
        rt_thread_startup(tid1);
    else {
        rt_kprintf("thread_103_create fail!\r\n");
    }
    tid2 = rt_thread_create("thread2",
                            duji_thread_entry, RT_NULL,
                            512,
                            25, 5);
    if (tid2 != RT_NULL)
            rt_thread_startup(tid2);
    else {
        rt_kprintf("duji_thread_create fail!\r\n");
    }
    tid3 = rt_thread_create("thread3",
                            rc522_thread_entry, RT_NULL,
                            512,
                            26, 5);
    if (tid3 != RT_NULL)
            rt_thread_startup(tid3);
    else {
        rt_kprintf("rc522_thread_create fail!\r\n");
    }
    tid4 = rt_thread_create("thread4",
                            bujin_thread_entry, RT_NULL,
                            512,
                            24, 5);
    if (tid4 != RT_NULL)
            rt_thread_startup(tid4);
    else {
        rt_kprintf("bujin_thread_create fail!\r\n");
    }
    tid6 = rt_thread_create("thread6",
                            wait_thread_entry, RT_NULL,
                            512,
                            26, 5);
    if (tid6 != RT_NULL)
            rt_thread_startup(tid6);
    else {
        rt_kprintf("wait_thread_create fail!\r\n");
    }
    tid5 = rt_thread_create("thread5",
                            run_thread_entry, RT_NULL,
                            512,
                            27, 5);
    if (tid5 != RT_NULL)
            rt_thread_startup(tid5);
    else {
        rt_kprintf("run_thread_create fail!\r\n");
    }
    /*tid7 = rt_thread_create("thread7",
                            esp_thread_entry, RT_NULL,
                            512,
                            27, 5);
    if (tid7 != RT_NULL)
            rt_thread_startup(tid7);
    else {
        rt_kprintf("esp_thread_create fail!\r\n");
    }*/

}
